#include "fintrf.h"
!     
!=======================================================================
! Gateway subroutine
subroutine mexfunction(nlhs, plhs, nrhs, prhs)

! Declarations
implicit none

! mexFunction arguments:
mwPointer plhs(*), prhs(*)
integer nlhs, nrhs

! Function declarations:
mwPointer mxGetPr
mwPointer mxCreateDoubleMatrix, mxCreateNumericArray
mwPointer mxGetM, mxGetN

! Pointers to input/output mxArrays:
mwPointer pr1 ,pr2 ,pr3 ,pr4 ,pr5 ,pr6 ,pr7 ,pr8 ,pr9 ,pr10,&
          pr11,pr12,pr13,pr14,pr15,pr16,pr17,pr18,pr19,pr20,&
          pr21,pr22,pr23,pr24,pr25,pr26,pr27,pr28,pr29,pr30,&
          pr31,pr32,&
          pr1_out,pr2_out,pr3_out,pr4_out,pr5_out,pr6_out,pr7_out

! Array information:
mwPointer Maturity,nx

! Arguments for mxCreateNumericArray
integer*4 mxClassIDFromClassName
integer*4 classid
integer*4 complexflag
mwSize ndim3,ndim4
mwSize dims3(3),dims4(4)

!-----------------------------------------------------------------------
! Check for proper number of arguments. 
if(nrhs .ne. 32) then
   call mexErrMsgIdAndTxt ('MATLAB:dblmat:nInput','32 input required.')
end if
if(nlhs .gt. 7) then
   call mexErrMsgIdAndTxt ('MATLAB:dblmat:nOutput','Too many output arguments.')
end if
         
! Create Fortran arrays from the input argument.
pr1  = mxGetPr(prhs(1))
pr2  = mxGetPr(prhs(2))
pr3  = mxGetPr(prhs(3))
pr4  = mxGetPr(prhs(4))
pr5  = mxGetPr(prhs(5))
pr6  = mxGetPr(prhs(6))
pr7  = mxGetPr(prhs(7))
pr8  = mxGetPr(prhs(8))
pr9  = mxGetPr(prhs(9))
pr10 = mxGetPr(prhs(10))
pr11 = mxGetPr(prhs(11))
pr12 = mxGetPr(prhs(12))
pr13 = mxGetPr(prhs(13))
pr14 = mxGetPr(prhs(14))
pr15 = mxGetPr(prhs(15))
pr16 = mxGetPr(prhs(16))
pr17 = mxGetPr(prhs(17))
pr18 = mxGetPr(prhs(18))
pr19 = mxGetPr(prhs(19))
pr20 = mxGetPr(prhs(20))
pr21 = mxGetPr(prhs(21))
pr22 = mxGetPr(prhs(22))
pr23 = mxGetPr(prhs(23))
pr24 = mxGetPr(prhs(24))
pr25 = mxGetPr(prhs(25))
pr26 = mxGetPr(prhs(26))
pr27 = mxGetPr(prhs(27))
pr28 = mxGetPr(prhs(28))
pr29 = mxGetPr(prhs(29))
pr30 = mxGetPr(prhs(30))
pr31 = mxGetPr(prhs(31))
pr32 = mxGetPr(prhs(32))

! Get the size of the input array.
call mxCopyPtrToInteger4(pr26, Maturity, 1)
call mxCopyPtrToInteger4(pr31, nx, 1)

! Create matrix for the return argument.
!for P
plhs(1) = mxCreateDoubleMatrix(Maturity,1, 0)   
pr1_out = mxGetPr(plhs(1))

!for Px
plhs(2) = mxCreateDoubleMatrix(Maturity,nx, 0)  
pr2_out = mxGetPr(plhs(2))

!for Pxx
classid = mxClassIDFromClassName('double')        
complexflag = 0                                 ! 0 for real data
ndim3    = 3
dims3(1) = Maturity
dims3(2) = nx
dims3(3) = nx
plhs(3) = mxCreateNumericArray(ndim3, dims3, classid, complexflag)
pr3_out = mxGetPr(plhs(3))

!for Pxxx
ndim4    = 4                                     
dims4(1) = Maturity
dims4(2) = nx
dims4(3) = nx
dims4(4) = nx
plhs(4) = mxCreateNumericArray(ndim4, dims4, classid, complexflag)
pr4_out = mxGetPr(plhs(4))

!Pss
plhs(5) = mxCreateDoubleMatrix(Maturity,1, 0)     
pr5_out = mxGetPr(plhs(5))

!Pssx
plhs(6) = mxCreateDoubleMatrix(Maturity,nx, 0)     
pr6_out = mxGetPr(plhs(6))

!Psss
plhs(7) = mxCreateDoubleMatrix(Maturity,1, 0)     
pr7_out = mxGetPr(plhs(7))

! Call the computational routine.
call fortranSub(%VAL(pr1_out),%VAL(pr2_out),%VAL(pr3_out),%VAL(pr4_out),%VAL(pr5_out),%VAL(pr6_out),%VAL(pr7_out),&
   %VAL(pr1) ,%VAL(pr2) ,%VAL(pr3) ,%VAL(pr4) ,%VAL(pr5) ,%VAL(pr6) ,%VAL(pr7) ,%VAL(pr8) ,%VAL(pr9) ,%VAL(pr10),&
   %VAL(pr11),%VAL(pr12),%VAL(pr13),%VAL(pr14),%VAL(pr15),%VAL(pr16),%VAL(pr17),%VAL(pr18),%VAL(pr19),%VAL(pr20),&
   %VAL(pr21),%VAL(pr22),%VAL(pr23),%VAL(pr24),%VAL(pr25),%VAL(pr26),%VAL(pr27),%VAL(pr28),%VAL(pr29),%VAL(pr30),&
   %VAL(pr31),%VAL(pr32))

return
end

!===============================================================

! Computational subroutine
subroutine fortranSub(P,Px,Pxx,Pxxx,Pss,Pssx,Psss,&
    M,Mxp,Myp,Mxpx,Mxpxp,Mxpy,Mxpyp,Mypx,Mypxp,Mypy,Mypyp,&  !11 terms
    gx,gxx,gxxx,gss,gssx,gsss,hx,hxx,hxxx,hss,hssx,hsss,eta,& !13 terms
    vectorMom3,Maturity,ny_1bond,order_app,solveRiskTerms,ny,nx,ne) !8 terms

IMPLICIT NONE
INTEGER, INTENT(IN) :: Maturity,ny_1bond,order_app,solveRiskTerms,ny,nx,ne
REAL*8, INTENT(OUT) :: P(Maturity,1),Px(Maturity,nx),Pxx(Maturity,nx,nx),&
                       Pxxx(Maturity,nx,nx,nx),Pss(Maturity,1),Pssx(Maturity,nx),Psss(Maturity,1)
REAL*8,  INTENT(IN) :: M(1,1),Mxp(1,nx),Myp(1,ny),Mxpx(nx,nx),Mxpxp(nx,nx),&
                       Mxpy(nx,ny),Mxpyp(nx,ny),Mypx(ny,nx),Mypxp(ny,nx),Mypy(ny,ny),Mypyp(ny,ny),&
                       gx(ny,nx),gxx(ny,nx,nx),gxxx(ny,nx,nx,nx),gss(ny,1),gssx(ny,nx),gsss(ny,1),&
                       hx(nx,nx),hxx(nx,nx,nx),hxxx(nx,nx,nx,nx),hss(nx,1),hssx(nx,nx),hsss(nx,1),&
                       eta(nx,ne),vectorMom3(ne)
! Declaring the remaining variables
INTEGER k,alfa1,alfa2,alfa3,gama1,gama2,phi1,phi2,beta1
REAL*8  tmp(nx,nx),trace(1,1),tmp_nene(ne,ne),Myp_gxx(nx,nx),tmp1(1,nx),tmp2(1,nx),eta_eta(nx,nx)
character*120 line

! To print to a file
!open(unit=6,file='tmp.txt',status='unknown')
!write(6,*) 'Maturity = ',Maturity, 'nx = ',nx,'ny = ',ny

! Initializing the arrays
P            = 0._8
Px           = 0._8
Pxx          = 0._8
Pxxx         = 0._8
Pss          = 0._8
Pssx         = 0._8
Psss         = 0._8

! Defining
eta_eta      = MATMUL(eta,TRANSPOSE(eta))

! The all the derivatives for the first bond price
P(1,1)       = M(1,1)
Px(1,:)      = gx(ny_1bond,:)
Pxx(1,:,:)   = gxx(ny_1bond,:,:)
Pxxx(1,:,:,:)= gxxx(ny_1bond,:,:,:)
Pss(1,1)     = gss(ny_1bond,1)
Pssx(1,:)    = gssx(ny_1bond,:)
Psss(1,1)    = gsss(ny_1bond,1)

! The value of bond prices in deterministic steady state steady state
DO k=2,Maturity
   P(k,1) = M(1,1)**k
END DO

! The first order terms
DO k=2,Maturity
   ! With matrix notation
   Px(k:k,:) = Px(1:1,:) + MATMUL(Px(k-1:k-1,:),hx)
END DO
! Stop if order_app = 1
IF (order_app == 1) THEN
   RETURN
END IF

! The second order terms, Pxx
DO k=2,Maturity
   tmp = 0._8
   DO gama1 = 1,nx
       tmp = tmp + Px(k-1,gama1)*hxx(gama1,:,:)
   END DO
   Pxx(k,:,:) = Pxx(1,:,:) + MATMUL(TRANSPOSE(hx),MATMUL(Pxx(k-1,:,:),hx)) + tmp
END DO
! The constant correction, Pss
DO k=2,Maturity
   tmp_nene = MATMUL(TRANSPOSE(eta),MATMUL(Pxx(k-1,:,:),eta))
   trace = 0._8
   DO phi1=1,ne
      trace = trace + tmp_nene(phi1,phi1)
   END DO
   Pss(k:k,1:1) = Pss(1:1,1:1) + Pss(k-1:k-1,1:1) + MATMUL(Px(k-1:k-1,:),hss) + trace&
                  + MATMUL(Px(k-1:k-1,:),MATMUL(eta_eta,TRANSPOSE(Px(k-1:k-1,:))))&
                  + 2._8*M**(-1._8)*MATMUL(Myp(1:1,:),MATMUL(gx,MATMUL(eta_eta,TRANSPOSE(Px(k-1:k-1,:)))))&
                  + 2._8*M**(-1._8)*MATMUL(Mxp(1:1,:),MATMUL(eta_eta,TRANSPOSE(Px(k-1:k-1,:))))
END DO
! Stop if order_app == 2
IF (order_app == 2) THEN
   RETURN
END IF

! The third order order terms, Pxxx
DO k=2,Maturity
   DO alfa1=1,nx
      DO alfa2=alfa1,nx
         DO alfa3=alfa2,nx
            tmp(1,1) = 0._8
            DO gama1=1,nx
               tmp(1:1,1:1) = tmp(1:1,1:1) + hx(gama1,alfa1)*MATMUL(TRANSPOSE(hx(:,alfa2:alfa2)),&
                                              MATMUL(Pxxx(k-1,gama1,:,:),hx(:,alfa3:alfa3)))
            END DO
            Pxxx(k:k,alfa1:alfa1,alfa2,alfa3) = Pxxx(1:1,alfa1:alfa1,alfa2,alfa3) + tmp(1:1,1:1)&
                    + MATMUL(TRANSPOSE(hx(:,alfa1:alfa1)),MATMUL(Pxx(k-1,:,:),hxx(:,alfa2:alfa2,alfa3)))&
                    + MATMUL(TRANSPOSE(hxx(:,alfa1:alfa1,alfa3)),MATMUL(Pxx(k-1,:,:),hx(:,alfa2:alfa2)))&
                    + MATMUL(TRANSPOSE(hxx(:,alfa1:alfa1,alfa2)),MATMUL(Pxx(k-1,:,:),hx(:,alfa3:alfa3)))&
                    + MATMUL(Px(k-1:k-1,:),hxxx(:,alfa1:alfa1,alfa2,alfa3))
                
            ! Using symmetry for alfa1 and alfa2
            IF (alfa1 == alfa2 .AND. alfa2 .NE. alfa3) THEN  !alfa1==alfa2~=alfa3
               !Pxxx(k,alfa1,alfa1,alfa3)= Pxxx(k,alfa1,alfa2,alfa3)
               Pxxx(k,alfa1,alfa3,alfa1) = Pxxx(k,alfa1,alfa2,alfa3)
               Pxxx(k,alfa3,alfa1,alfa1) = Pxxx(k,alfa1,alfa2,alfa3)
            END IF
            ! Using symmetry for alfa2 and alfa3
            IF (alfa1 .NE. alfa2 .AND. alfa2 == alfa3) THEN   !alfa1~=alfa2==alfa3
               !Pxxx(k,alfa1,alfa2,alfa2)= Pxxx(k,alfa1,alfa2,alfa3)
               Pxxx(k,alfa2,alfa1,alfa2) = Pxxx(k,alfa1,alfa2,alfa3)
               Pxxx(k,alfa2,alfa2,alfa1) = Pxxx(k,alfa1,alfa2,alfa3)
            END IF
            ! Using symmetry for alfa1,alfa2, and alfa3
            IF (alfa1 .NE. alfa2 .AND. alfa1 .NE. alfa3 .AND. alfa2 .NE. alfa3) THEN !alfa1~=alfa2~=alfa3
               !Pxxx(k,alfa1,alfa2,alfa3) = Pxxx(k,alfa1,alfa2,alfa3)
               Pxxx(k,alfa1,alfa3,alfa2) = Pxxx(k,alfa1,alfa2,alfa3)
               Pxxx(k,alfa3,alfa1,alfa2) = Pxxx(k,alfa1,alfa2,alfa3)
               Pxxx(k,alfa3,alfa2,alfa1) = Pxxx(k,alfa1,alfa2,alfa3)
               Pxxx(k,alfa2,alfa3,alfa1) = Pxxx(k,alfa1,alfa2,alfa3)
               Pxxx(k,alfa2,alfa1,alfa3) = Pxxx(k,alfa1,alfa2,alfa3)
            END IF
         END DO
      END DO
   END DO
END DO

IF (solveRiskTerms == 0) THEN
   RETURN
END IF 

! The third order order terms, Pssx
! First some auxiliary variables
Myp_gxx = 0._8
DO gama1=1,nx
   DO gama2=1,nx
      Myp_gxx(gama1:gama1,gama2:gama2) = MATMUL(Myp(1:1,:),gxx(:,gama1:gama1,gama2))
   END DO
END DO
DO k=2,Maturity
   tmp1 = 0._8
   DO beta1=1,ny
      tmp1 = tmp1 + 2._8*M(1,1)**(-1._8)*Myp(1,beta1)*&
                 MATMUL(Px(k-1:k-1,:),MATMUL(eta_eta,MATMUL(gxx(beta1,:,:),hx)))
   END DO
   tmp2 = 0._8
   DO gama1=1,nx
      tmp2 = tmp2 + MATMUL(eta(gama1:gama1,:),MATMUL(TRANSPOSE(eta),MATMUL(Pxxx(k-1,gama1,:,:),hx)))
   END DO
   Pssx(k:k,:) = &
               -2._8*M(1,1)**(-1._8)*MATMUL(Myp,MATMUL(gx,MATMUL(eta_eta,MATMUL(TRANSPOSE(Px(k-1:k-1,:)),Px(1:1,:))))) &   !1)
               -2._8*M(1,1)**(-1._8)*MATMUL(Mxp,MATMUL(eta_eta,MATMUL(TRANSPOSE(Px(k-1:k-1,:)),Px(1:1,:)))) &                !2)
               +Pssx(1:1,:) &                                                                                                !3)
               +2._8*M(1,1)**(-1._8)*MATMUL(Px(k-1:k-1,:),MATMUL(eta_eta,MATMUL(TRANSPOSE(gx),&                              !4)
                 (MATMUL(Mypyp,MATMUL(gx,hx)) + MATMUL(Mypy,gx) + MATMUL(Mypxp,hx) + Mypx))))&
               +tmp1 &                                                                                                       !5)
               +2._8*M(1,1)**(-1._8)*MATMUL(Px(k-1:k-1,:),MATMUL(eta_eta,&                                                   !6)
                 (MATMUL(Mxpyp,MATMUL(gx,hx))+MATMUL(Mxpy,gx)+MATMUL(Mxpxp,hx)+Mxpx))) &
               +2._8*M(1,1)**(-1._8)*MATMUL(Myp,MATMUL(gx,MATMUL(eta_eta,MATMUL(Pxx(k-1,:,:),hx)))) &                        !7)
               +2._8*M(1,1)**(-1._8)*MATMUL(Mxp,MATMUL(eta_eta,MATMUL(Pxx(k-1,:,:),hx))) &                                   !8)
               +MATMUL(Px(k-1:k-1,:),MATMUL(eta_eta,MATMUL(Pxx(k-1,:,:),hx))) &                                              !9)
               +MATMUL(Px(k-1:k-1,:),MATMUL(eta_eta,MATMUL(Pxx(k-1,:,:),hx))) &                                              !10)
               +tmp2 &                                                                                                       !11)
               +MATMUL(TRANSPOSE(hss),MATMUL(Pxx(k-1,:,:),hx)) &                                                             !12)
               +MATMUL(Px(k-1:k-1,:),hssx) &                                                                                 !13)
               +MATMUL(Pssx(k-1:k-1,:),hx)                                                                                   !14)
END DO

IF (SUM(vectorMom3) == 0._8) THEN
   RETURN
END IF
! The third order order terms, Psss
DO k=2,Maturity
   ! Term b1)
   Psss(k,1) = Psss(1,1)                                                                                                     !b1

   ! The terms b2), b3), b5), b6), b7), b8) and b9)
   tmp(1,1) = 0._8
   DO phi1=1,ne
      tmp(1:1,1:1) = tmp(1:1,1:1) &
                  + 3._8*M(1,1)**(-1._8)*MATMUL(TRANSPOSE(MATMUL(gx,eta(:,phi1:phi1))*vectorMom3(phi1)),&                            !b2)
                      MATMUL(Mypyp,MATMUL(gx,eta(:,phi1:phi1))))*MATMUL(Px(k-1:k-1,:),eta(:,phi1:phi1)) &
                  + 6._8*M(1,1)**(-1._8)*MATMUL(TRANSPOSE(MATMUL(gx,eta(:,phi1:phi1))*vectorMom3(phi1)),&  	                     !b3)
                      MATMUL(Mypxp,eta(:,phi1:phi1)))*MATMUL(Px(k-1:k-1,:),eta(:,phi1:phi1))&
                  + 3._8*M(1,1)**(-1._8)*MATMUL(TRANSPOSE(eta(:,phi1:phi1)*vectorMom3(phi1)),&                                       !b5)
                      MATMUL(Mxpxp,eta(:,phi1:phi1)))*MATMUL(Px(k-1:k-1,:),eta(:,phi1:phi1))&
                  + 3._8*M(1,1)**(-1._8)*(MATMUL(Myp(1:1,:),MATMUL(gx,eta(:,phi1:phi1))) + MATMUL(Mxp(1:1,:),eta(:,phi1:phi1)))&                                           !b6)
                     *vectorMom3(phi1)*MATMUL(Px(k-1:k-1,:),eta(:,phi1:phi1))&
                     *MATMUL(Px(k-1:k-1,:),eta(:,phi1:phi1))&
                  + 3._8*M(1,1)**(-1._8)*(MATMUL(Myp(1:1,:),MATMUL(gx,eta(:,phi1:phi1))) + MATMUL(Mxp(1:1,:),eta(:,phi1:phi1)))&     !b7)
                     *vectorMom3(phi1)&
                     *MATMUL(TRANSPOSE(eta(:,phi1:phi1)),MATMUL(Pxx(k-1,:,:),eta(:,phi1:phi1)))&
                  + MATMUL(Px(k-1:k-1,:),eta(:,phi1:phi1))*MATMUL(Px(k-1:k-1,:),eta(:,phi1:phi1))&                                   !b8)
                     *MATMUL(Px(k-1:k-1,:),eta(:,phi1:phi1))*vectorMom3(phi1)&
                  + 3._8*MATMUL(Px(k-1:k-1,:),eta(:,phi1:phi1))&                                                                     !b9)
                     *MATMUL(TRANSPOSE(eta(:,phi1:phi1)*vectorMom3(phi1)),MATMUL(Pxx(k-1,:,:),eta(:,phi1:phi1)))
   END DO
   Psss(k,1) = Psss(k,1) + tmp(1,1)
    
   ! Term b4)
   tmp(1,1) = 0._8
   DO beta1=1,ny
      DO phi1=1,ne
         tmp(1:1,1:1) = tmp(1:1,1:1) + 3._8*M(1,1)**(-1._8)*&
                         MATMUL(TRANSPOSE(eta(:,phi1:phi1)*vectorMom3(phi1)*Myp(1,beta1)),MATMUL(gxx(beta1,:,:),eta(:,phi1:phi1)))&
                             *MATMUL(Px(k-1:k-1,:),eta(:,phi1:phi1))
      END DO
   END DO
   Psss(k,1) = Psss(k,1) + tmp(1,1)
    
   ! The term 10b)
   tmp(1,1) = 0._8
   DO gama1=1,nx
      DO phi1=1,ne
         tmp(1:1,1:1) = tmp(1:1,1:1) + MATMUL(TRANSPOSE(eta(:,phi1:phi1)),MATMUL(Pxxx(k-1,gama1,:,:),eta(:,phi1:phi1)))&
                                       *eta(gama1,phi1)*vectorMom3(phi1)
      END DO
   END DO
   Psss(k,1) = Psss(k,1) + tmp(1,1)
 
   ! Terms b11 and b12
   tmp(1:1,1:1) =  MATMUL(Px(k-1:k-1,:),hsss(:,1:1)) &                                                                !b11)
         + Psss(k-1:k-1,1:1)                                                                                         !b12)
   Psss(k,1) = Psss(k,1) + tmp(1,1)
END DO

END SUBROUTINE fortranSub

